#pragma once
// ============================================================================
// 文件：include/SLAM/matcher.hpp
// 说明：基于匹配点对（当前帧反光柱（base） ↔ 全局地标（map））求解 SE(2) 位姿。
//      - 要求匹配数 ≥ 2（工程上为稳定性通常 ≥ 3 才发布位姿）。
//      - 使用质心去中心化 + SVD 简化（二维下解析形式）。
//      - 输出：map_T_base 的平面位姿（x, y, yaw）。
// ============================================================================

#include "SLAM/common.hpp"
#include "HungarianAlgorithm.hpp"

namespace SLAM {
struct MatcherParams {
  double mahalanobis_gate2{9.0};    // d^2 门限（~3σ^2）
  double huge_cost{1e6};            // 超门限时的惩罚
  double obs_sigma_xy{0.08};        // 观测噪声（m）
};

class MahalanobisMatcher {
public:
  explicit MahalanobisMatcher(const MatcherParams& p): p_(p) {}

  // 输入：全局地标（含协方差）与本帧观测，输出匹配对（允许不匹配）
  std::vector<MatchPair> match(const std::vector<Landmark>& lms,
                               const std::vector<Cov2>& lm_covs,
                               const std::vector<Reflector>& obs) const
  {
    std::vector<MatchPair> matches; matches.reserve(seeds.size() + hgra.size());

    if (lms.empty() || obs.empty()) return matches;

    // (1) 一次性马氏预门：缩小候选集合（代价用 d^2）
    GatedCost GC = build_gated_cost(obs, lms, lm_covs);

    // (2) 最近邻粗配（仅在通过马氏预门的候选里做互为最近邻）
    std::vector<MatchPair> seeds = mutual_nearest_pairs(GC);
    // 标记已占用的 obs/lm，剩余的交给匈牙利
    std::vector<bool> obs_used(obs_map.size(), false), lm_used(lms_confirm.size(), false);
    for (auto &p : seeds) { obs_used[p.obs_index] = true; lm_used[p.lm_index] = true; }

    // (3) 为剩余未占用的 obs/lm 构建“缩小版”代价矩阵并调用匈牙利（代价仍用 d^2）
    std::vector<int> row2obs, col2lm; // 小矩阵->原索引映射
    row2obs.reserve(obs_map.size()); col2lm.reserve(lms_confirm.size());
    for (size_t i=0;i<obs_map.size();++i) if (!obs_used[i]) row2obs.push_back((int)i);
    for (size_t j=0;j<lms_confirm.size();++j) if (!lm_used[j]) col2lm.push_back((int)j);

    std::vector<std::vector<double>> cost_small(row2obs.size(),
          std::vector<double>(col2lm.size(), std::numeric_limits<double>::infinity()));
    for (size_t ri=0; ri<row2obs.size(); ++ri) {
      int i = row2obs[ri];
      for (size_t cj=0; cj<col2lm.size(); ++cj) {
        int j = col2lm[cj];
        cost_small[ri][cj] = GC.ok[i][j] ? GC.cost[i][j] : std::numeric_limits<double>::infinity();
      }
    }

    // 调用你现有的匈牙利实现（代价=马氏 d^2；无效=+inf）
    std::vector<int> assign_small;  // 每一行匹配到的列索引，或 -1
    HungarianAlgorithm hungarian;
    hungarian.Solve(cost_small, assign_small, p_.huge_cost);

    // 收集匈牙利结果（只收仍在门限内的）
    std::vector<MatchPair> hgra;
    for (size_t ri=0; ri<assign_small.size(); ++ri) {
      int cj = assign_small[ri];
      if (cj < 0) continue;
      int i = row2obs[ri], j = col2lm[cj];
      if (GC.ok[i][j]) { hgra.push_back({i, j, GC.cost[i][j]}); }
    }

    // (4) “最近邻粗配 + 匈牙利补全”的合并结果 → 最终二次门（可更严/结合欧氏阈）
    auto push_pair = [&](const MatchPair& p){
      MatchPair mp; mp.obs_index = p.obs_index; mp.lm_index = lms_confirm[p.lm_index].id; mp.d2 = p.d2;
      // 二次门（可比预门更严，比如 95%→99%）
      const double strict_gate2 = std::max(matcher_params_.mahalanobis_gate2, 9.21); // 9.21≈χ²_2,0.99
      Eigen::Vector2d z(obs_map[mp.obs_index].x, obs_map[mp.obs_index].y);
      Eigen::Vector2d mu(lms_confirm[p.lm_index].x, lms_confirm[p.lm_index].y);
      Eigen::Matrix2d Pm; 
      Pm << lm_covs[p.lm_index].xx, lm_covs[p.lm_index].xy, lm_covs[p.lm_index].yx, lm_covs[p.lm_index].yy;
      Eigen::Matrix2d R = R_from_range_intensity(obs_map[mp.obs_index]);
      double d2 = mahalanobis_d2(z, mu, Pm, R);
      if (d2 <= strict_gate2) matches.push_back(mp);
    };

    // 合并：先放 seeds（互为最近邻的高置信），再补充匈牙利的剩余
    for (auto &p:seeds) push_pair(p);
    for (auto &p:hgra)  push_pair(p);
    return matches;
  }

private:
  MatcherParams p_;
};


inline bool solveSE2(const std::vector<Reflector>& obs_base,
                     const std::vector<Landmark>&  sel_map,
                     geometry_msgs::msg::Pose&     mTb_out,
                     double&                       rmse)
{
  if (obs_base.size() != sel_map.size() || obs_base.size() < 2) return false;

  // 1) 计算各自质心
  double cx_o=0, cy_o=0, cx_m=0, cy_m=0;
  for (size_t i=0;i<obs_base.size();++i) {
    cx_o += obs_base[i].x; cy_o += obs_base[i].y;
    cx_m += sel_map[i].x;  cy_m += sel_map[i].y;
  }
  cx_o/=obs_base.size(); cy_o/=obs_base.size();
  cx_m/=sel_map.size();  cy_m/=sel_map.size();

  // 2) 计算二维相关矩阵分量
  double Sxx=0,Sxy=0,Syx=0,Syy=0;
  for (size_t i=0;i<obs_base.size();++i) {
    const double ox = obs_base[i].x - cx_o;
    const double oy = obs_base[i].y - cy_o;
    const double mx = sel_map[i].x  - cx_m;
    const double my = sel_map[i].y  - cy_m;
    Sxx += ox*mx; Sxy += ox*my; Syx += oy*mx; Syy += oy*my;
  }

  // 3) 旋转角（二维下解析解）
  double theta = std::atan2(Sxy - Syx, Sxx + Syy);
  const double c = std::cos(theta), s = std::sin(theta);

  // 4) 平移（把 base 质心旋转后对齐到 map 质心）
  const double tx = cx_m - (c*cx_o - s*cy_o);
  const double ty = cy_m - (s*cx_o + c*cy_o);

  // 5) 计算 RMSE（用于调试/门控）
  double err=0;
  for (size_t i=0;i<obs_base.size();++i) {
    double px = c*obs_base[i].x - s*obs_base[i].y + tx;
    double py = s*obs_base[i].x + c*obs_base[i].y + ty;
    err += std::pow(px - sel_map[i].x,2) + std::pow(py - sel_map[i].y,2);
  }
  rmse = std::sqrt(err / obs_base.size());

  // 6) 输出 map->base 位姿
  mTb_out.position.x = tx; mTb_out.position.y = ty; mTb_out.position.z = 0.0;
  tf2::Quaternion q; q.setRPY(0,0,theta); mTb_out.orientation = tf2::toMsg(q);
  return true;
}

} // namespace SLAM
